#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_interfaces/action/robot_collection.hpp"
#include "robot_interfaces/msg/robot_state.hpp"
#include "robot_interfaces/msg/robot_target.hpp"
#include <cmath>
#include <math.h>
#include <queue>
#include <vector>
class RobotTracter : public rclcpp::Node {
  using GoalHandle = rclcpp_action::ClientGoalHandle<
      robot_interfaces::action::RobotCollection>;

public:
  RobotTracter(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "Hello, I am RobotTracter!");
    this->declare_parameter<std::string>("target_frame","target");
    this->declare_parameter<float>("window_size", 5);
    data_subscriber_ =
        this->create_subscription<robot_interfaces::msg::RobotState>(
            "data", 10,
            std::bind(&RobotTracter::data_callback, this,
                      std::placeholders::_1));
    target_publisher_ =
        this->create_publisher<robot_interfaces::msg::RobotTarget>("target",
                                                                   10);
    this->client_ptr = rclcpp_action::create_client<
        robot_interfaces::action::RobotCollection>(this, "robot_collection");
  }
  void send_goal() {
    using namespace std::placeholders;
    if (!this->client_ptr->wait_for_action_server(std::chrono::seconds(15))) {
      RCLCPP_ERROR(this->get_logger(), "service not available");
      rclcpp::shutdown();
      return;
    }
    auto goal_msg = robot_interfaces::action::RobotCollection::Goal();
    goal_msg.whether_destory = true;
    RCLCPP_INFO(this->get_logger(), "Sending goal");
    auto send_goal_options = rclcpp_action::Client<
        robot_interfaces::action::RobotCollection>::SendGoalOptions();
    send_goal_options.goal_response_callback =
        std::bind(&RobotTracter::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
        std::bind(&RobotTracter::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
        std::bind(&RobotTracter::result_callback, this, _1);
    this->client_ptr->async_send_goal(goal_msg, send_goal_options);
  }

private:
  int destory_timer_count = 0;
  rclcpp_action::Client<robot_interfaces::action::RobotCollection>::SharedPtr
      client_ptr;
  void goal_response_callback(GoalHandle::SharedPtr goal_handle) {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "goal rejected");
    } else {
      RCLCPP_INFO(this->get_logger(), "goal accepted");
    }
  }
  void feedback_callback(
      GoalHandle::SharedPtr,
      const std::shared_ptr<
          const robot_interfaces::action::RobotCollection::Feedback>
          feedback) {
    RCLCPP_INFO(this->get_logger(), "the distance_to_robot is %f",
                feedback->distance_to_robot);
  }
  void result_callback(const GoalHandle::WrappedResult &result) {
    switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      RCLCPP_INFO(this->get_logger(), "goal succeeded");
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "goal aborted");
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(this->get_logger(), "goal canceled");
      return;
    default:
      RCLCPP_ERROR(this->get_logger(), "goal result unknown");
      return;
    }
    RCLCPP_INFO(this->get_logger(), "result is %s",
                result.result->succeed_collect ? "true" : "false");
  }
  rclcpp::Subscription<robot_interfaces::msg::RobotState>::SharedPtr
      data_subscriber_;
  void data_callback(const robot_interfaces::msg::RobotState::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Received data");
    std::vector<float> sum = {0, 0, 0, 0};
    int window_size;
    this->get_parameter("window_size", window_size);

    this->filter_target_base_x.push_back(msg->target_base_x);
    this->filter_target_base_y.push_back(msg->target_base_y);
    this->filter_base_world_x.push_back(msg->base_world_x);
    this->filter_base_world_y.push_back(msg->base_world_y);
    count++;
    if (count < window_size) {
      if (count < (window_size) / 2) {
        filter_out_data[1].push_back(filter_target_base_x[count]);
        filter_out_data[2].push_back(filter_target_base_y[count]);
        filter_out_data[3].push_back(filter_base_world_x[count]);
        filter_out_data[4].push_back(filter_base_world_y[count]);
      }
    } else {

      for (int j = count; j > count - window_size; j--) {
        sum[1] += filter_target_base_x[j];
        sum[2] += filter_target_base_y[j];
        sum[3] += filter_base_world_x[j];
        sum[4] += filter_base_world_y[j];
      }
      filter_out_data[1].push_back(sum[1] / window_size);
      filter_out_data[2].push_back(sum[2] / window_size);
      filter_out_data[3].push_back(sum[3] / window_size);
      filter_out_data[4].push_back(sum[4] / window_size);
      sum = {0, 0, 0, 0};
    }
    if (count >= window_size) {

      this->target_base_x.push(filter_out_data[1][count - window_size]);
      this->target_base_y.push(filter_out_data[2][count - window_size]);
      this->base_world_x.push(filter_out_data[3][count - window_size]);
      this->base_world_y.push(filter_out_data[4][count - window_size]);

      if ((count - window_size) != 0) {
        base_x.push(base_world_x.front());
        base_y.push(base_world_y.front());
        target_x.push(target_base_x.front() + base_world_x.front());
        target_y.push(target_base_y.front() + base_world_y.front());
        base_x.push(base_world_x.back());
        base_y.push(base_world_y.back());
        target_x.push(target_base_x.back() + base_world_x.back());
        target_y.push(target_base_y.back() + base_world_y.back());
        this->arm_v = sqrt(pow(target_y.back() - target_y.front(), 2) +
                           pow(target_x.back() - target_x.front(), 2)) /
                      time_step;
        float tan_theta_squ = std::pow((target_y.back() - target_y.front()) /
                                           (target_x.back() - target_x.front()),
                                       2);
        send_target(target_x.back(), target_y.back(), base_x.back(),
                    base_y.back(), tan_theta_squ);
        if (sqrt(pow(target_x.back() - target_x.front(), 2) +
                 pow(target_y.back() - target_y.front(), 2)) < 0.25) {
          destory_timer_count++;
          if (destory_timer_count > 200) {
            send_goal();
          }
          target_base_x.pop();
          target_base_y.pop();
          base_world_x.pop();
          base_world_y.pop();
        }
      }
    }
  }
  void send_target(float x, float y, float base_x, float base_y,
                   float tan_theta_squ) {
    robot_interfaces::msg::RobotTarget target_msg;
    target_msg.target_world_v = arm_v;
    target_msg.target_world_x = x;
    target_msg.target_world_y = y;
    target_msg.base_world_x = base_x;
    target_msg.base_world_y = base_y;
    target_msg.target_tan_theta_squ = tan_theta_squ;
    target_publisher_->publish(target_msg);
  }

  rclcpp::Publisher<robot_interfaces::msg::RobotTarget>::SharedPtr
      target_publisher_;

  int count = 0;
  float v = 20;
  float time_step = 0.02;
  float arm_v = 0;

  std::vector<std::vector<float>> filter_out_data;
  std::vector<float> filter_target_base_x;
  std::vector<float> filter_target_base_y;
  std::vector<float> filter_base_world_x;
  std::vector<float> filter_base_world_y;
  std::queue<float> target_base_x;
  std::queue<float> target_base_y;
  std::queue<float> base_world_x;
  std::queue<float> base_world_y;
  std::queue<float> base_x;
  std::queue<float> base_y;
  std::queue<float> target_x;
  std::queue<float> target_y;
};
int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<RobotTracter>("robot_tracter");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
